// This file is part of OpenMeca, an easy software to do mechanical simulation.
//
// Author(s)    :  - Damien ANDRE  <openmeca@gmail.com>
//
// Copyright (C) 2012 Damien ANDRE
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.


// This source file was inspired of the "libGeometrical" from 
// the GranOO workbench : http://www.granoo.org and 
// the qglviewer library : http://www.libqglviewer.com



#include <cmath>
#include <iostream>

#include "OpenMeca/Geom/Frame.hpp"
#include "OpenMeca/Geom/Point.hpp"
#include "OpenMeca/Geom/Vector.hpp"
#include "OpenMeca/Geom/Quaternion.hpp"


namespace OpenMeca
{
  namespace Geom 
  {
 
    template<> const Point<_3D>       Frame<_3D>::GlobalCenter = Point<_3D>();
    template<> const Quaternion<_3D>  Frame<_3D>::GlobalQuaternion= Quaternion<_3D>();
    template<> const Frame<_3D>       Frame<_3D>::Global = Frame<_3D>(Frame<_3D>::GlobalCenter, Frame<_3D>::GlobalQuaternion);
    
    template<> const Point<_3D>       Frame<_3D>::NullCenter = Point<_3D>();
    template<> const Quaternion<_3D>  Frame<_3D>::NullQuaternion= Quaternion<_3D>();
    template<> const Frame<_3D>       Frame<_3D>::Null  = Frame<_3D>(Frame<_3D>::NullCenter, Frame<_3D>::NullQuaternion); 
    
  

    template<> void
    Frame<_3D>::DeleteAxis()
    {
      delete xAxis_;
      delete yAxis_;
      delete zAxis_;
    }
    
    template<> void
    Frame<_3D>::BuildAxis()
    {
      boost::function<const Frame<_3D>& ()> f = boost::bind(&Frame<_3D>::GetMe, boost::ref(*this));
      xAxis_ = new Vector<_3D>(1.,0.,0.,f);
      yAxis_ = new Vector<_3D>(0.,1.,0.,f);
      zAxis_ = new Vector<_3D>(0.,0.,1.,f);
    }

  

    template<>
    void
    Frame<_3D>::UpdateGLMatrix()
    {
      if (GetRank()==0)
	{
	  m_[0][0] = 1.;
	  m_[1][0] = 0.;
	  m_[2][0] = 0.;
	  m_[0][1] = 0.;
	  m_[1][1] = 1.;
	  m_[2][1] = 0.;
	  m_[0][2] = 0.;
	  m_[1][2] = 0.;
	  m_[2][2] = 1.;
	  m_[0][3] = 0.;
	  m_[1][3] = 0.;
	  m_[2][3] = 0.;
	  m_[3][0] = 0.;
	  m_[3][1] = 0.;
	  m_[3][2] = 0.;
	  m_[3][3] = 1.;
	  //Translation 
	  m_[3][0] = 0.;
	  m_[3][1] = 0.;
	  m_[3][2] = 0.;
	}
      else
	{
	  
	  Quaternion<_3D> quat(GetQuaternion(), &Frame<_3D>::GetGlobal);
	  //Rotation with quaternion
	  const double q00 = 2. * quat[0] * quat[0];
	  const double q11 = 2. * quat[1] * quat[1];
	  const double q22 = 2. * quat[2] * quat[2];
	  const double q01 = 2. * quat[0] * quat[1];
	  const double q02 = 2. * quat[0] * quat[2];
	  const double q03 = 2. * quat[0] * quat.GetReal();
	  const double q12 = 2. * quat[1] * quat[2];
	  const double q13 = 2. * quat[1] * quat.GetReal();
	  const double q23 = 2. * quat[2] * quat.GetReal();
	  m_[0][0] = 1. - q11 - q22;
	  m_[1][0] = q01 - q23;
	  m_[2][0] = q02 + q13;
	  m_[0][1] = q01 + q23;
	  m_[1][1] = 1. - q22 - q00;
	  m_[2][1] = q12 - q03;
	  m_[0][2] = q02 - q13;
	  m_[1][2] = q12 + q03;
	  m_[2][2] = 1. - q11 - q00;
	  m_[0][3] = 0.;
	  m_[1][3] = 0.;
	  m_[2][3] = 0.;
	  m_[3][0] = 0.;
	  m_[3][1] = 0.;
	  m_[3][2] = 0.;
	  m_[3][3] = 1.;
	  //Translation with point
	  Point<_3D> point(GetCenter(), &Frame<_3D>::GetGlobal);
	  m_[3][0] = point[0];
	  m_[3][1] = point[1];
	  m_[3][2] = point[2];
	}
    }

    template<>
    void
    Frame<_3D>::UpdateGLMatrix() const
    {
      Frame<_3D>* me = const_cast< Frame<_3D>* >(this);
      me->UpdateGLMatrix();
    }
 
  }
}
